
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms.c
  * @author     baiyang
  * @date       2021-8-12
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <common/grapilot.h>

#include <rtthread.h>
#include <rtdevice.h>
#include <rthw.h>
/*-----------------------------------macro------------------------------------*/
#define FMS_EVENT_LOOP        (1<<0)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static char thread_fms_stack[1024*10];
struct rt_thread thread_fms_handle;

static struct rt_timer fms_timer;
static struct rt_event fms_event;

Fms_handle fms;

static ahrs_view _ahrs;
static AttitudeMulti_ctrl _atc;
static Position_ctrl _psc;
static MotorsMat_HandleTypeDef _motor;

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       飞行管理模块构造函数
  * @param[in]   
  * @param[out]  
  * @retval      
  * @note        构造函数中函数的调用顺序不要随意更改
  */
void fms_ctor()
{
    fms.G_Dt = PERIOD_MS_100HZ*0.001f;

    fms_param_assign();

    // 飞行模式构造函数要在赋值飞行模式指针之前调用
    mode_ctor();

    fms.prev_control_mode = STABILIZE;
    fms._last_reason = MODE_REASON_UNKNOWN;
    fms.control_mode_reason = MODE_REASON_UNKNOWN;

    fms.flightmode = get_mode_stabilize();
    fms.mode_stabilize = get_mode_stabilize();
    fms.mode_althold = get_mode_althold();

    fms_init_rc_in();

    fms.ahrs = &_ahrs;
    fms.motors = (Motors_HandleTypeDef *)&_motor;
    fms.attitude_control = (Attitude_ctrl *)&_atc;
    fms.pos_control = &_psc;

    lpf_set_cutoff1_vec3f(&fms.land_accel_ef_filter, 1.0f);

    MotorsMatrix((MotorsMat_HandleTypeDef *)fms.motors,1000/PERIOD_MS_200HZ,400);

    attctrl_multi_init(fms.attitude_control, fms.motors, PERIOD_MS_200HZ*0.001f);
    attctrl_set_dt(fms.attitude_control, PERIOD_MS_100HZ*0.001f);

    posctrl_ctor(fms.pos_control, fms.ahrs, fms.motors, fms.attitude_control, PERIOD_MS_100HZ*0.001f);

    fms_arming_ctor(&fms.arming);
}

/**
  * @brief       1Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fast_one_hz_loop()
{
    fms_param_update();
    
    fms_arming_update(&fms.arming);

    if (!fms.motors->_armed) {
        MotorsMat_set_frame_class_and_type((MotorsMat_HandleTypeDef *)fms.motors,
            (motor_frame_class)PARAM_GET_INT8(VEHICLE,FRAME_CLASS),
            (motor_frame_type)PARAM_GET_INT8(VEHICLE,FRAME_TYPE));

        // set all throttle channel settings
        MotorsMC_set_throttle_range((MotorsMC_HandleTypeDef *)fms.motors,PARAM_GET_INT16(RC,RC_MIN),PARAM_GET_INT16(RC,RC_MAX));
    }

}

/**
  * @brief       5Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fast_fives_hz_loop()
{
    fms_update_origin();
}

/**
  * @brief       10Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fast_ten_hz_loop()
{
    RCs_read_aux_all(&(fms.channels));
    fms_lost_vehicle_check();
    fms_arm_motors_check();
    fms_auto_disarm_check();
}

/**
  * @brief       25Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fast_twenty_five_hz_loop()
{

}

/**
  * @brief       50Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fast_fifty_hz_loop()
{
    fms_update_throttle_mix();
    fms_update_auto_armed();
}

/**
  * @brief       100Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fast_one_hundred_hz_loop()
{
    mavproxy_loop();
    fms_read_radio();
    RCs_read_mode_switch(&(fms.channels));

    fms_update_flight_mode();
    fms_update_throttle_hover();
    fms_standby_update();
}

/**
  * @brief       200Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fast_two_hundred_hz_loop()
{
    attctrl_rate_controller_run(fms.attitude_control);
    fms_motors_output();
    fms_update_land_and_crash_detectors();
}

/**
  * @brief       200Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        用于更新需要最先更新的数据
  */
static void fast_fast_loop()
{
    fms_update_ahrs();
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_loop()
{
    uint32_t NowMs = time_millis();

    TIMETAG_CHECK_EXECUTE2(fast,PERIOD_MS_200HZ,NowMs,fast_fast_loop();)

    TIMETAG_CHECK_EXECUTE2(100Hz,PERIOD_MS_100HZ,NowMs,fast_one_hundred_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(200Hz,PERIOD_MS_200HZ,NowMs,fast_two_hundred_hz_loop();)

    TIMETAG_CHECK_EXECUTE2(50Hz,PERIOD_MS_50HZ,NowMs,fast_fifty_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(25Hz,PERIOD_MS_25HZ,NowMs,fast_twenty_five_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(10Hz,PERIOD_MS_10HZ,NowMs,fast_ten_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(5Hz,PERIOD_MS_5HZ,NowMs,fast_fives_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(1Hz,PERIOD_MS_1HZ,NowMs,fast_one_hz_loop();)
}

/**
  * @brief       定时器回调函数，发送定时器事件
  * @param[in]   parameter  
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_time_update(void* parameter)
{
    rt_event_send(&fms_event, FMS_EVENT_LOOP);
}

/**
  * @brief       飞行任务线程入口函数
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_entry(void *parameter)
{
    rt_err_t res;
    rt_uint32_t recv_set = 0;
    rt_uint32_t wait_set = FMS_EVENT_LOOP;

    /* create event */
    res = rt_event_init(&fms_event, "fms", RT_IPC_FLAG_FIFO);
    
    /* register timer event */
    rt_timer_init(&fms_timer, "fms",
                    fms_time_update,
                    RT_NULL,
                    rt_tick_from_millisecond(1),
                    RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_HARD_TIMER);
    rt_timer_start(&fms_timer);
    
    while(1)
    {
        res = rt_event_recv(&fms_event, wait_set, RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, 
                                RT_WAITING_FOREVER, &recv_set);
        
        if(res == RT_EOK){
            if(recv_set & FMS_EVENT_LOOP){
                fms_loop();
            }
        }
    }
}

gp_err task_fms_init(void)
{
    rt_err_t res;

    fms_ctor();

    res = rt_thread_init(&thread_fms_handle,
                           "fms",
                           fms_entry,
                           RT_NULL,
                           &thread_fms_stack[0],
                           sizeof(thread_fms_stack),PRIORITY_FMS,5);

    RT_ASSERT(res == RT_EOK);

    if (res == RT_EOK) {
        rt_thread_startup(&thread_fms_handle);
    }

    return res;
}

/*------------------------------------test------------------------------------*/


